// **********************************************************
//  *
//  *  Copyright (c) 2003  SeikoEpson Inc
//  *  All Rights Reserved
//  *
//  *  File name : f_SIN.S
//  *  Function  :
//  *      C Standard Library Function "sin"
//  *      The "sin" function calculates the sine of x which is radian.
//  *      This file is copied from math.lib of 
//  *                  CC33 tool(CC33v40.exe + CCPA45.exe, 
//  *                  math of ansilib33v25 )
//  *
//  *  original file's Revision  :
//  *      1999/12/08  Created by H.Ishida
//  *  Revision  :
//  *      2003/04/04  IrumaSoft M.Takeishi   start
//  *      2003/06/27  IrumaSoft M.Takeishi   changed for gnu33 and 
//  *                                          got more paformance
//  *
//  **********************************************************/
// 
//  --- Format of floating point (double precision) ---
// 
//   31|30         23|22                0
//    -----------------------------------
//   | |  exponent   |   fraction        |
//    -----------------------------------
//   |    8 bits     |    23 bits        |

#include	"f_mathf.def"

// ==============================================================================
// 	Function: float f_sin(float x)
// 
// 	Input:  %r6: Argument x in radian float.
// 
// 	Output: %r4: Return value, sine of x in float.
// 
	.text
	.align	1
	.global	f_sin

f_sin:

	//  NaN or INF check
	ld.w	%r7,%r6
	xld.w	%r8,0x7f800000
	and	%r7,%r8
	cmp	%r7, %r8
	xjrne	_GoodArgument
	//  set errno and NaN
	xld.w	%r4, 0x7fc00000
	
	xld.w	%r7,EDOM
	xld.w	[errno],%r7				//  data pointer is needed
	ret

_GoodArgument:

	pushn	%r3						//  Save registers %r3...%r0.
	sub	%sp,1						//  Presearve working area in five words.

	//  %r3     = Taylor series's sign parameter (float).
	//  %r2     = argument x.
	//  %r1     = Quadrant of trigonometric function (word, unsigned long).
	//  %r0     = temporary push register
	// [%sp+00] = 1nd argument of f_modf, pointer to float data.

	ld.w	%r2,%r6					//  argument x.

// ##### REMOVED 12/08/1999 #############
// # // 
// # // 	Clear the sign bit of argument x in %r3:%r2 by shifting
// # // 	to make it a plus value.
// # // 
// # 	sll		%r3,1				//  Shift the argument x to the left  one bit.
// #	srl		%r3,1				//  Shift the argument x to the right one bit.
// ##### REMOVE END #####################

	ld.w	%r1,0x0					//  Initialize the quadrant to 0.

	sra		%r6,1					//  Check if x is minus?
	xjrge	_ArgIsNotMinus			//  Branch if plus or zero.
// 
// 	--- if the argument x is minus ---
// 	Clear the sign bit of argument x in %r3:%r2 by shifting
// 	to make it a plus value.
// 
	sll		%r2,1					//  Shift the argument x to the left  one bit.
	srl		%r2,1					//  Shift the argument x to the right one bit.

	ld.w	%r1,0x2					//  Set the quadrant 2.
// 
// 	Mutiply the argument x by the constant 2*Pai
// 
_ArgIsNotMinus:
	ld.w	%r6,%r2					//  Argument x (low).
	xld.w	%r7,F_TWO_PAI			//  Multiplier, constant 2*Pai (low).
									//  
	xcall	__mulsf3				//  Calculate multiplication//  argument x = x * (2 * Pai).
									//  The result product is %r5:%r4.
	ld.w	%r2,%r4					//  Put result x in %r2 (low).
// 
// 	Check if the argument x is large radian.
// 	If large, do some argument reduction, else skip reduction procedure.
// 
	ld.w	%r6,%r2					//  Argument x.
	xld.w	%r7,LARGE_RAD			//  Large radian constant.
									//  Compare argument x and the large radian.
	xcall	__fcmps					//  to check if the argument x is large.
	xjrle	_ArgIsNotLarge			//  Skip if the argument x is not large.
// 
// 	--- Argument x is large ---
// 	Reduce the argument to avoid overflow.
// 
	ld.w	%r6,%r2					//  Argument x, 1st parameter of f_modf().
	ld.w	%r7,%sp					//  Pointer to the fraction part [%sp+00], the 2nd parameter of f_modf().
	xcall	f_modf					//  Calculate the integer part and the fraction part.
// 
// 	Use the integer part as a Tayler series's sign parameter.
// 
	ld.w	%r3,%r4					//  Tayler series's sign parameter.
// 
// 	Use the fraction part as a new argument x.
// 
	xld.w	%r2,[%sp]				//  Copy fraction result to the argument x.

// 
// 	Adjust the new argument 
// 	by adding the quadrant.
// 
	ld.w	%r6,%r1					//  Load the quadrant integer.
									//  
	xcall	__floatsisf				//  Convert the quadrant to a float value.
					// 
	ld.w	%r6,%r2					//  Argument x.
	ld.w	%r7,%r4					//  Quadrant in float.
					// 
	xcall	__addsf3				//  Add the argument x and the quadrant.
									//  The result is %r4.
	ld.w	%r2,%r4					//  Put the result back to the argument x.


// # 		modf( 0.25 * x, &f )// 

	ld.w	%r6,%r2					//  Argument x.
	xld.w	%r7,QUARTER				//  Constant 0.25 in float.
									// 
	xcall	__mulsf3				//  Calculate multiplication x*0.25.
									// 
	ld.w	%r6,%r4					//  Set x*0.25 result as 1st param of f_modf.
	ld.w	%r7,%sp					//  2nd param of f_modf, pointer to the fraction.
									// 
	xcall	f_modf					//  call f_MODF, another library function.
// 
// 	Calculate the new quadrant
// 	by subtracting 4.0 from the argument x
// 	and then multiplying by the fraction part of the argument.
// 	(argument x - 4.0 * fraction)
// 
	xld.w	%r6,[%sp]				//  Load the fraction part.
	xld.w	%r7,FOUR				//  Set a constant 4.0.
									//  
	xcall	__mulsf3				//  Caculate fraction*4.0.
									//  
	ld.w	%r6,%r2					//  Set the argument x.
	ld.w	%r7,%r4					//  Set the result of fraction*4.0.
									//  
	xcall	__subsf3				//  Calculate fraction*4.0-x.
									//  
	ld.w	%r6,%r4					//  Put the result back to the argument x.
									//  
	xcall	__fixsfsi				//  Make it into an integer (resulting quadrant).
									//  
	ld.w    %r1,%r4					//  Store the new quadrant.
	cmp		%r1,0
	xjp	_ChkIfQuadrant1or3			//  Jump to the next process,
									//  completing the adjustment, 
									//  when the argument x is large.
// 
// 	--- Argument x is not large ---
// 
// 	Obtain the integer part of the argument x in float
// 	to use it for adjusting the Taylor series's sign parameter
// 	accoring to the quadrant.
// 
_ArgIsNotLarge:
	ld.w	%r6,%r2					//  Argument x.
									//  
	xcall	__fixsfsi				//  Cast the argument x in float to an integer.
									//  %r4 <- Result integer.
	ld.w	%r0,%r4					//  Keep the integer part of the argument x in %r0.
// 
// 	Obtain the fraction part of the argument x 
// 	by subtracting the integer part.
// 
	ld.w	%r6,%r0					//  Set the integer part value as input to "__floatsisf".
									//  
	xcall	__floatsisf				//  Cast the integer part back to a float value.
									//  
	ld.w	%r6,%r2					//  Set the resulting float as subtrahend of __subsf3.
	ld.w	%r7,%r4					//  Set the result of former __floatsisf as the next subtracter.
									//  
	xcall	__subsf3				//  Subtract %13 (argument x) from %r6 (integer argument).
									//  
	ld.w	%r3,%r4					//  Set the subtraction result back to the argument x.
// 
// 	Calculate the new quadtrant
// 	by adding the integer part of the argument x
// 	and then logically anding with a constant 3.
// 
	ld.w	%r10,%r1				//  Quadrant integer.
	add		%r1,%r0					//  Adding integer part and the argument x.
	and		%r1,0x3					//  Anding it with a consant 3.
									//  Put the result as a new quadrant.
// 
// 	--- Checkng according to the quadrant ---
// 	Check if the quadrant is 1 or 3 (odd number).
// 
_ChkIfQuadrant1or3:
	ld.w	%r4,%r1					//  Load the quadrant
	and		%r4,0x1					//  Check if the quadrant is 1 or 3?
	xjreq	_ChkIfQuadMoreThan1		//  Skip if the quadrant is neither 1 nor 3.
// 
// 	--- Quadrant is 1 or 3 ---
// 	Adjust the Taylor series's sign parameter
// 	by calculating "param = 1 - param".
	//  If changed here, precision becomes much better.
// 
	xld.w	%r6,ONE					//  Set constant 1.0.
	ld.w	%r7,%r3					//  Taylor series's sign parameter.
									//  
	xcall	__subsf3				//  Subtract %r6 - %r7, Result is %r4
									//  
	ld.w	%r3,%r4					//  Store the result of calculation.
// 
// 	--- Checking if the quadrant is larger than 1 ---
// 
_ChkIfQuadMoreThan1:
	ld.w	%r10,%r1				//  Loading the quadrant.

	xcmp	%r10,1					//  Check if it is larger than 1?
	xjrle	_SquareTaylorParam		//  If not larger than 1, skip to complete calculating Tayler's param.
// 
// 	--- Quadrant is larger than 1 ---
// 	then, negate the Taylor series's sign parameter.
// 
	ld.w	%r7,%r3					//  Load the Tayler series's parameter.

	XXOR	%r7,SIGN_MASK			//  Negate it.
	ld.w	%r3,%r7					//  Store it.
// 	
// 	--- Calculate the square of the Tayler series's sign parameter ---
// 	Complete adjusting the Taylor series's sign parameter
// 	by calculating its square.
// 
_SquareTaylorParam:
	ld.w	%r6,%r3					//  Multiplicand, the argument.
	ld.w	%r7,%r3					//  Multiplicand, the argument.
									//  
	xcall	__mulsf3				//  Calculate square, the argument x * argument x
									//  
	ld.w	%r2,%r4					//  Put the square result in the argument x.
// 
// 	
// 
// 	(((((P4 * x + P3) * x + P2) * x + P1) * x + P0) * y)
// 	----------------------------------------------------
// 	           (((x + Q2) * x + Q1) * x + Q0)
// 
	ld.w	%r6,%r2					//  Argument square x.
	xld.w	%r7,CP4					//  float 2.80782741762206855540e0.
									//  
	xcall	__mulsf3				//  
									//  
	ld.w	%r6,%r4					//  
	xld.w	%r7,CP3					//  float -2.37859324578121572813e2.
									//  
	xcall	__addsf3				//  
									//  
	ld.w	%r6,%r4					//  
	ld.w	%r7,%r2					//  
									//  
	xcall	__mulsf3				//  
									//  
	ld.w	%r6,%r4					//  
	xld.w	%r7,CP2					//  float 7.06413608140068845387e3.
									//  
	xcall	__addsf3				//  
									//  
	ld.w	%r6,%r4					//  
	ld.w	%r7,%r2					//  
									//  
	xcall	__mulsf3				//  
									//  
	ld.w	%r6,%r4					//  
	xld.w	%r7,CP1					//  float -7.65864156388469564263e4.
									//  
	xcall	__addsf3				//  
									//  
	ld.w	%r6,%r4					//  
	ld.w	%r7,%r2					//  
									//  
	xcall	__mulsf3				//  
									//  
	ld.w	%r6,%r4					//  
	xld.w	%r7,CP0					//  float 2.07823684169610118261e5.
									//  
	xcall	__addsf3				//  
									//  
	ld.w	%r6,%r4					//  
	ld.w	%r7,%r3					//  
									//  
	xcall	__mulsf3				//  
									//  
	ld.w	%r0,%r4					//  
	ld.w	%r6,%r2					//  
	xld.w	%r7,CQ2					//  float 1.08999811037129049396e2.
									//  
	xcall	__addsf3				//  
									//  
	ld.w	%r6,%r4					//  
	ld.w	%r7,%r2					//  
									//  
	xcall	__mulsf3				//  
									//  
	ld.w	%r6,%r4					//  
	xld.w	%r7,CQ1					//  float 5.65168679531691759621e3.
									//  
	xcall	__addsf3				//  
									//  
	ld.w	%r6,%r4					//  
	ld.w	%r7,%r2					//  
									//  
	xcall	__mulsf3				//  
									//  
	ld.w	%r6,%r4					//  
	xld.w	%r7,CQ0					//  float 1.32304666508649301250e5.
									//  
	xcall	__addsf3				//  
									//  
	ld.w	%r6,%r0					//  
	ld.w	%r7,%r4					//  
									//  
	xcall	__divsf3				//  

	add		%sp,1					//  Roll up the working area.
	popn	%r3						//  Restore registers %r3...%r0.
	ret								//  

